package com.rockwell.maze800;

import java.util.ArrayDeque;
import java.util.Date;
import java.util.Deque;
import java.util.concurrent.Executor;
import java.util.concurrent.Executors;

import net.wimpi.modbus.facade.ModbusSerialMaster;
import net.wimpi.modbus.usbserial.SerialParameters;

import org.opencv.core.Point;

import com.rockwell.maze800.modbus.ModbusSerialClient;
import com.rockwell.maze800.model.BarrierDetector;
import com.rockwell.maze800.model.IRobotLogger;
import com.rockwell.maze800.model.LeftFirstMazeSolver;
import com.rockwell.maze800.model.Maze;
import com.rockwell.maze800.model.Robot;
import com.rockwell.maze800.model.RobotCommander;
import com.rockwell.maze800.model.RobotSensors;
import com.rockwell.maze800.model.TrashDetector;
import com.rockwell.maze800.model.TreasureDetector;
import com.rockwell.maze800.views.MazeCameraView;
import com.rockwell.maze800.views.OnOrientationChangeListener;
import com.rockwell.maze800.views.OnColorBlobDetectListener;

import android.os.AsyncTask;
import android.os.Bundle;
import android.app.Activity;
import android.content.Context;
import android.hardware.usb.UsbManager;
import android.text.Editable;
import android.text.SpannableString;
import android.text.TextWatcher;
import android.text.style.UnderlineSpan;
import android.view.Menu;
import android.view.View;
import android.view.View.OnClickListener;
import android.widget.Button;
import android.widget.EditText;
import android.widget.ScrollView;
import android.widget.TextView;
import android.widget.Toast;

public class MaintenanceActivity extends Activity {
	private Robot 			m_robot;
    private ModbusSerialClient m_modbusMaster;
	private MazeCameraView	m_mazeCameraView;
	private Executor m_executor;
	
	private IRobotLogger m_logger = new IRobotLogger() {

		@Override
		public void log(final String msg) {
			MaintenanceActivity.this.runOnUiThread(new Runnable(){
			    public void run(){
			    	//Toast.makeText(MaintenanceActivity.this, msg, Toast.LENGTH_SHORT).show();
			    	TextView logDisplay = (TextView)findViewById(R.id.text_log);
			    	logDisplay.append(msg + "\n");
			    }
			});
		}

		@Override
		public void log(final Exception ex) {
			MaintenanceActivity.this.runOnUiThread(new Runnable(){
			    public void run(){
			    	Toast.makeText(MaintenanceActivity.this, ex.getMessage(), Toast.LENGTH_SHORT).show();
			    }
			});
		}

		@Override
		public void onRobotPaused() {
		}

		@Override
		public void onRobotResumed() {
		}

		@Override
		public void onRobotIdle() {
		}
    };
			
	private OnOrientationChangeListener m_orientationListener = new OnOrientationChangeListener() {
        public void onOrientationChanged(float[] orientation) {
        	m_robot.setOrientation(orientation[0]);
        }
    };
    
    private OnColorBlobDetectListener m_treasureListener = new OnColorBlobDetectListener() {
        public void onDetected(boolean present, Point center, float radius, int length, int width) {
        	m_robot.setTreasure(present, center, radius, length, width);
        }
    };

	@Override
	protected void onCreate(Bundle savedInstanceState) {
		super.onCreate(savedInstanceState);
		setContentView(R.layout.activity_maintenance);
		
		m_mazeCameraView = (MazeCameraView) this.findViewById(R.id.maze_camera_view);
		m_mazeCameraView.setTeachMode(false);
		m_mazeCameraView.setOrientationChangeListener(m_orientationListener);
		m_mazeCameraView.setTreasureDetectListener(m_treasureListener);
		
		SerialParameters params = new SerialParameters();
		params.setPortName("COM1");
		params.setBaudRate(19200);
		params.setDatabits(8);
		params.setParity("None");
		params.setStopbits(1);
		params.setEncoding("ascii");
		params.setEcho(false);

		UsbManager usb = (UsbManager) getSystemService(Context.USB_SERVICE);
    	m_modbusMaster = new ModbusSerialClient(usb);
		
    	final ScrollView sv = (ScrollView)findViewById(R.id.scrollview_log);
    	TextView tv = (TextView)findViewById(R.id.text_log);
    	tv.addTextChangedListener(new TextWatcher() {

			@Override
			public void afterTextChanged(Editable arg0) {
				sv.fullScroll(ScrollView.FOCUS_DOWN);
			}

			@Override
			public void beforeTextChanged(CharSequence s, int start, int count,
					int after) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void onTextChanged(CharSequence s, int start, int before,
					int count) {
				// TODO Auto-generated method stub
				
			}
    		
    	});
    	
    	
		setupButtons();
	}

	@Override
	public boolean onCreateOptionsMenu(Menu menu) {
		// Inflate the menu; this adds items to the action bar if it is present.
		getMenuInflater().inflate(R.menu.robot, menu);
		return true;
	}

	@Override
	public void onStart() {
		super.onStart();
		
		try {
			m_executor = Executors.newFixedThreadPool(1);
	
			double width = SettingsActivity.getRobotWidth(this);
			double length = SettingsActivity.getRobotLength(this);
			
			RobotSensors sensors = new RobotSensors(m_modbusMaster);
			RobotCommander commander = new RobotCommander(m_modbusMaster);
			TreasureDetector treasureDetector = new TreasureDetector();
			BarrierDetector barrierDetector = new BarrierDetector();
			TrashDetector trashDetector = new TrashDetector();
			
			m_robot = new Robot(width, length, sensors, commander, 
					new Maze(30, 5), new LeftFirstMazeSolver(), treasureDetector, barrierDetector, trashDetector);
			m_robot.setLogger(m_logger, 5);
			m_robot.start(false);
		}
		catch (Exception ex) {
            Toast.makeText(this, ex.getMessage(), Toast.LENGTH_SHORT).show();
		}
	}
	
	@Override
	public void onStop() {
		super.onStop();
		m_mazeCameraView.onDeactivated();
	}
	
	@Override
	public void onResume() {
    	try {
			m_modbusMaster.connect("COM1");
		} catch (Exception e) {
			Toast.makeText(this, "Failed to connect to the modbus slave", Toast.LENGTH_LONG).show();
		}
    	
		super.onResume();
		m_mazeCameraView.onActivated();
	}
	
	@Override
    public void onPause()
    {
		m_modbusMaster.disconnect();
        super.onPause();
        m_mazeCameraView.onDeactivated();
    }

	private void setupButtons() {
		Button startJog = (Button)findViewById(R.id.button_test_start_jog);
		
		startJog.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				RobotTask robotTask = new RobotTask(MaintenanceActivity.this, new Runnable() {

					@Override
					public void run() {
						m_robot.testJog();
					}
					
				});
				
				robotTask.executeOnExecutor(m_executor);
			}
			
		});
		
		Button stop = (Button)findViewById(R.id.button_test_stop);
		
		stop.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				RobotTask robotTask = new RobotTask(MaintenanceActivity.this, new Runnable() {

					@Override
					public void run() {
						m_robot.testStop();
					}
					
				});
				
				robotTask.executeOnExecutor(m_executor);
			}
			
		});
		
		Button turnLeft = (Button)findViewById(R.id.button_test_turn_left);
		
		turnLeft.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				RobotTask robotTask = new RobotTask(MaintenanceActivity.this, new Runnable() {

					@Override
					public void run() {
						Deque<Robot.RunEnum> actions = new ArrayDeque<Robot.RunEnum>(4);
						actions.add(Robot.RunEnum.PERFORM_JOG);
						actions.add(Robot.RunEnum.PERFORM_TURN_LEFT);
						actions.add(Robot.RunEnum.PERFORM_JOG);
						
						m_robot.testContinueMission(actions);
					}
					
				});
				
				robotTask.executeOnExecutor(m_executor);
			}
			
		});
		
		Button turnRight = (Button)findViewById(R.id.button_test_turn_right);
		
		turnRight.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				RobotTask robotTask = new RobotTask(MaintenanceActivity.this, new Runnable() {

					@Override
					public void run() {
						Deque<Robot.RunEnum> actions = new ArrayDeque<Robot.RunEnum>(4);
						actions.add(Robot.RunEnum.PERFORM_JOG);
						actions.add(Robot.RunEnum.PERFORM_TURN_RIGHT);
						actions.add(Robot.RunEnum.PERFORM_JOG);
						
						m_robot.testContinueMission(actions);
					}
					
				});
				
				robotTask.executeOnExecutor(m_executor);
			}
			
		});
		
		Button uTurn = (Button)findViewById(R.id.button_test_uturn);
		
		uTurn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				RobotTask robotTask = new RobotTask(MaintenanceActivity.this, new Runnable() {

					@Override
					public void run() {
						
						Deque<Robot.RunEnum> actions = new ArrayDeque<Robot.RunEnum>(4);
						actions.add(Robot.RunEnum.PERFORM_JOG);
						actions.add(Robot.RunEnum.PERFORM_TURN_BACK);
						actions.add(Robot.RunEnum.PERFORM_JOG);
						
						m_robot.testContinueMission(actions);
					}
					
				});
				
				robotTask.executeOnExecutor(m_executor);
			}
			
		});
		
		Button jog2 = (Button)findViewById(R.id.button_test_jog2);
		
		jog2.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				RobotTask robotTask = new RobotTask(MaintenanceActivity.this, new Runnable() {

					@Override
					public void run() {
						m_robot.testJog2();
					}
					
				});
				
				robotTask.executeOnExecutor(m_executor);
			}
			
		});
		
		Button armdown = (Button)findViewById(R.id.button_test_arm_down);
		
		armdown.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				RobotTask robotTask = new RobotTask(MaintenanceActivity.this, new Runnable() {

					@Override
					public void run() {
						m_robot.testArmDown();
					}
					
				});
				
				robotTask.executeOnExecutor(m_executor);
			}
			
		});
		
		Button pick = (Button)findViewById(R.id.button_test_pick);
		
		pick.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				RobotTask robotTask = new RobotTask(MaintenanceActivity.this, new Runnable() {

					@Override
					public void run() {
						m_robot.testPick();
					}
					
				});
				
				robotTask.executeOnExecutor(m_executor);
			}
			
		});
		
		Button pickSequence = (Button)findViewById(R.id.button_test_pick_sequence);
		
		pickSequence.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				RobotTask robotTask = new RobotTask(MaintenanceActivity.this, new Runnable() {

					@Override
					public void run() {
						Deque<Robot.RunEnum> actions = new ArrayDeque<Robot.RunEnum>(4);
						actions.add(Robot.RunEnum.PERFORM_JOG);
						actions.add(Robot.RunEnum.PERFORM_PICK);
						actions.add(Robot.RunEnum.PERFORM_JOG);
						
						m_robot.testContinueMission(actions);
					}
					
				});
				
				robotTask.executeOnExecutor(m_executor);
			}
			
		});
		
		Button turn = (Button)findViewById(R.id.button_test_turn);
		
		turn.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				EditText angleSource = (EditText)findViewById(R.id.editText_turn_angle);
				String angleText = angleSource.getText().toString();
				
				if (angleText != null && !angleText.isEmpty()) {
					final int angle = Integer.parseInt(angleText);
					
					RobotTask robotTask = new RobotTask(MaintenanceActivity.this, new Runnable() {

						@Override
						public void run() {
							m_robot.testTurn(angle);
						}
						
					});
					
					robotTask.executeOnExecutor(m_executor);
				}
			}
			
		});
		
		Button move = (Button)findViewById(R.id.button_test_move);
		
		move.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				EditText distanceSource = (EditText)findViewById(R.id.editText_move_distance);
				String distanceText = distanceSource.getText().toString();
				
				if (distanceText != null && !distanceText.isEmpty())
				{
					final int distance = Integer.parseInt(distanceText);
					
					RobotTask robotTask = new RobotTask(MaintenanceActivity.this, new Runnable() {

						@Override
						public void run() {
							m_robot.testMove(distance);
						}
					});
					
					robotTask.executeOnExecutor(m_executor);
				}
			}
			
		});
	}
	
	private void enableButtons(boolean enabled) {
		Button startJog = (Button)findViewById(R.id.button_test_start_jog);
		startJog.setEnabled(enabled);

		Button stop = (Button)findViewById(R.id.button_test_stop);
		stop.setEnabled(enabled);
		
		Button turnLeft = (Button)findViewById(R.id.button_test_turn_left);
		turnLeft.setEnabled(enabled);
		
		Button turnRight = (Button)findViewById(R.id.button_test_turn_right);
		turnRight.setEnabled(enabled);
		
		Button armDown = (Button)findViewById(R.id.button_test_arm_down);
		armDown.setEnabled(enabled);
		
		Button pick = (Button)findViewById(R.id.button_test_pick);
		pick.setEnabled(enabled);
		
		Button turn = (Button)findViewById(R.id.button_test_turn);
		turn.setEnabled(enabled);
		
		Button move = (Button)findViewById(R.id.button_test_move);
		move.setEnabled(enabled);
	}
	
	static class RobotTask extends AsyncTask<Void, Void, String> {
		MaintenanceActivity m_activity;
		Runnable m_task;
		
		public RobotTask(MaintenanceActivity activity, Runnable task) {
			m_activity = activity;
			m_task = task;
		}
		
		@Override
		protected void onPreExecute() {
			m_activity.enableButtons(false);
		}
		
		@Override
		protected String doInBackground(Void... params) {
			try {
				m_task.run();
				
				return "Succeeded";
			}
			catch (Exception ex) {
				return "Failed. " + ex.getMessage();
			}
		}
		
		@Override
		protected void onPostExecute(String result) {
			m_activity.enableButtons(true);
			
			Toast.makeText(m_activity, result, Toast.LENGTH_LONG).show();
		}
	}
}
